import numpy as np
from typing import Optional
from math_utils import Vector2, line_with_circle, line_with_triangle


class Obstacle:
    '''障碍物类, 支持圆形和三角形两种类型'''

    CIRCLE = 0
    '''圆形障碍物类型标识'''

    TRIANGLE = 1
    '''三角形障碍物类型标识'''

    def __init__(self, type: int, center: Optional[Vector2] = None, radius: Optional[float] = None, vertices: 'Optional[list[Vector2]]' = None) -> None:
        '''
        初始化障碍物对象, 推荐使用类方法 `circle()` 或 `triangle()` 来创建实例
        Args:
            type: 障碍物类型, 0 代表圆形, 1 代表三角形
            center: 圆形障碍物的圆心
            radius: 圆形障碍物的半径
            vertices: 三角形障碍物的顶点列表
        '''
        self.type = type

        if self.type == self.CIRCLE:
            if not all([center, radius]):
                raise Exception(f'圆形障碍物需要 center 和 radius 参数, 现在分别为 {center} 和 {radius}')
            self.center = center
            self.radius = radius

        elif self.type == self.TRIANGLE:
            if not vertices:
                raise Exception(f'三角形障碍物需要 vertices 参数, 现在为 {vertices}')
            elif len(vertices) != 3:
                raise Exception(f'vertices 参数长度应当为 3, 现在为 {len(vertices)}')
            self.vertices = vertices

        else:
            raise Exception(f'未知的障碍物类型: {type}')

    @classmethod
    def circle(cls, center: Vector2, radius: float) -> 'Obstacle':
        '''创建一个圆形障碍物'''
        return cls(cls.CIRCLE, center=center, radius=radius)

    @classmethod
    def triangle(cls, v1: Vector2, v2: Vector2, v3: Vector2) -> 'Obstacle':
        '''创建三角形障碍物'''
        return cls(cls.TRIANGLE, vertices=[v1, v2, v3])

    def collide_with_line(self, v1: Vector2, v2: Vector2) -> bool:
        '''判断线段是否与障碍物相交'''
        if self.type == self.CIRCLE:
            if self.center is None or self.radius is None:
                return False
            return line_with_circle(self.center, self.radius, v1, v2, True)

        elif self.type == self.TRIANGLE:
            if self.vertices is None:
                return False
            # 检查端点是否在三角形内部
            if self.is_point_inside(v1) or self.is_point_inside(v2):
                return True
            # 检查线段是否与三角形边相交
            return line_with_triangle(self.vertices[0], self.vertices[1], self.vertices[2], v1, v2, segment=True)

        return False

    def point_in_circle(self, xx: np.ndarray, yy: np.ndarray) -> np.ndarray:
        '''判断一组点是否在圆形内 (numpy 向量化计算)'''
        if self.center is None or self.radius is None:
            return np.zeros_like(xx, dtype=bool)
        return (xx - self.center.x)**2 + (yy - self.center.y)**2 < self.radius**2

    def is_point_inside(self, p: Vector2) -> bool:
        '''判断单个点是否在障碍物内部'''
        if self.type == self.CIRCLE:
            if self.center is None or self.radius is None:
                return False
            return Vector2.euclidean_distance_2(self.center, p) < self.radius**2

        elif self.type == self.TRIANGLE:
            if self.vertices is None:
                return False
            return self._point_in_triangle_single(p)

        return False

    def _point_in_triangle_single(self, p: Vector2) -> bool:
        '''使用重心坐标法判断点是否在三角形内'''
        v0 = self.vertices[2] - self.vertices[0]
        v1 = self.vertices[1] - self.vertices[0]
        v2 = p - self.vertices[0]

        dot00 = v0.dot(v0)
        dot01 = v0.dot(v1)
        dot02 = v0.dot(v2)
        dot11 = v1.dot(v1)
        dot12 = v1.dot(v2)

        denom = dot00 * dot11 - dot01 * dot01
        if abs(denom) < 1e-9:
            return False

        inv_denom = 1 / denom
        u = (dot11 * dot02 - dot01 * dot12) * inv_denom
        v = (dot00 * dot12 - dot01 * dot02) * inv_denom

        return (u >= 0) and (v >= 0) and (u + v <= 1)

    def point_in_triangle(self, xx: np.ndarray, yy: np.ndarray) -> np.ndarray:
        '''判断一组点是否在三角形内 (numpy 向量化计算)'''
        if self.vertices is None:
            return np.zeros_like(xx, dtype=bool)

        # 计算重心坐标
        v0 = self.vertices[2] - self.vertices[0]
        v1 = self.vertices[1] - self.vertices[0]
        v2_xx = xx - self.vertices[0].x
        v2_yy = yy - self.vertices[0].y

        # 计算点积
        dot00 = v0.dot(v0)
        dot01 = v0.dot(v1)
        dot02 = v0.x * v2_xx + v0.y * v2_yy
        dot11 = v1.dot(v1)
        dot12 = v1.x * v2_xx + v1.y * v2_yy

        # 计算重心坐标
        denom = dot00 * dot11 - dot01 * dot01
        inv_denom = 1 / denom

        u = (dot11 * dot02 - dot01 * dot12) * inv_denom
        v = (dot00 * dot12 - dot01 * dot02) * inv_denom

        return (u >= -1e-6) & (v >= -1e-6) & (u + v <= 1 + 1e-6)

    def __repr__(self) -> str:
        '''返回障碍物的字符串表示'''
        if self.type == self.CIRCLE:
            return f'圆形: 圆心={self.center}, 半径={self.radius}'
        elif self.type == self.TRIANGLE:
            return '三角形: 顶点=' + ', '.join(map(str, self.vertices))
        return f'未知障碍物类型: {self.type}'


if __name__ == '__main__':
    # 创建测试障碍物
    obstacle_list = [
        Obstacle.circle(Vector2(3, 5), 1),
        Obstacle.triangle(Vector2(2, 2), Vector2(3, 3), Vector2(2, 3)),
        Obstacle.triangle(Vector2(2, 2), Vector2(3, 3), Vector2(3, 2)),
        Obstacle.triangle(Vector2(5, 5), Vector2(8, 8), Vector2(2, 8)),
    ]

    print("障碍物信息:")
    for obstacle in obstacle_list:
        print(f'  {obstacle}')

    # 测试线段与障碍物的碰撞检测
    test_lines = [
        (Vector2(0, 0), Vector2(4, 6)),
        (Vector2(3, 5), Vector2(4, 6)),
        (Vector2(1, 1), Vector2(2, 2)),
        (Vector2(6, 6), Vector2(7, 7)),
    ]

    print("\n线段与障碍物碰撞检测:")
    for i, (p1, p2) in enumerate(test_lines, 1):
        print(f"  测试线段 {i}: {p1} - {p2}")
        for j, obstacle in enumerate(obstacle_list, 1):
            collide = obstacle.collide_with_line(p1, p2)
            obstacle_type = '圆形' if obstacle.type == Obstacle.CIRCLE else '三角形'
            status = '碰撞' if collide else '未碰撞'
            print(f"    与障碍物 {j} ({obstacle_type}): {status}")
